/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "MapDisplay.hpp"
#include <GL/glut.h>

CMapDisplay::CMapDisplay()
:mapClient(NULL),
//connected(false),
//opened(false),
showHeightMap(true),
showOccu(true),
showLines(true),
showAreas(true),
z(0),
y_orientation(1),
occuThreshold(0.05),
detailLevel(KHighDetail)
{
  mapperMutex=ownMutex_Init();
}

CMapDisplay::~CMapDisplay()
{
  
}

void CMapDisplay::Draw()
{
  glPushAttrib(GL_ALL_ATTRIB_BITS);
  glPushMatrix();
  
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  
  //if (opened)
  //{
    /*to do here
    -create display list for updated blocks
    --different lists for different detail levels
    --remember to empty updated block vectors!
    -display block and lines
    --display only blocks that are inside viewable area
    */
    ownMutex_Lock(mapperMutex);
    
    //create display lists for blocks that have changed
    if (updatedGroundBlocks.size())
    {
      gmbVector::iterator i;
      for (i=updatedGroundBlocks.begin();i!=updatedGroundBlocks.end();i++)
      {
        createHeightMapDisplayList(*i);
      }
      updatedGroundBlocks.clear();
    }
    
    if (updatedOccuBlocks.size())
    {
      occuVector::iterator i;
      for (i=updatedOccuBlocks.begin();i!=updatedOccuBlocks.end();i++)
      {
        createOccuDisplayList(*i);
      }
      updatedOccuBlocks.clear();
    }
    
    gmbMap *gmap=mapper.GetGroundModel();
    gmbMap::const_iterator gi;
    occuMap *omap=mapper.GetOccuModel();
    occuMap::const_iterator oi;
    
    //draw blocks
    for (gi=gmap->begin();gi!=gmap->end();gi++)
    {
      if (isDrawn(gi->second))
      {
        displayHeightMapBlock(gi->second);
      }
    }
    
    for (oi=omap->begin();oi!=omap->end();oi++)
    {
      if (isDrawn(oi->second))
      {
        displayOccuBlock(oi->second);
      }
    }
      
    //draw lines
    if (showLines)
      {
      glLineWidth(2);
      glColor3f(0.2, 0.2, 1.0);
      glBegin(GL_LINES);
      std::vector<MaCI::Map::TMapLine>::iterator li;
      for (li=mapper.GetMapLines()->begin(); li!=mapper.GetMapLines()->end();li++)
      {
        glVertex3f(li->x1,li->y1,0);
        glVertex3f(li->x2,li->y2,0);
      }
      glEnd();
    }
    
    //draw areas
    if (showAreas)
    {
      glLineWidth(2);
      glColor3f(1.0, 1.0, 0.0);
      
      std::vector<MaCI::Map::TArea>::iterator ai;
      for (ai=mapper.GetAreas()->begin(); ai!=mapper.GetAreas()->end();ai++)
      {
        std::vector<MaCI::Map::TVertex>::iterator vi;
        glBegin(GL_LINE_LOOP);
        for (vi=ai->vertices.begin(); vi!=ai->vertices.end();vi++)
          glVertex3f(vi->x,vi->y,0);
        glEnd();
      }
    }
    
    ownMutex_Unlock(mapperMutex);
    
  /*} else if (connected) {
    glColor3f(1,0,0);
    glBegin(GL_LINES);
    glVertex2f(-1.0,-1.0);
    glVertex2f(1.0,1.0);
          
    glVertex2f(-1.0,1.0);
    glVertex2f(1.0,-1.0);
    glEnd();
  }*/
  
  glPopAttrib();
  glPopMatrix();
}
/*
bool CMapDisplay::Connect(gimi::GIMI *gimi, int gimnetID, int minor)
{
  connected=true;
  
  mapClient= new MaCI::Map::CMapClient(gimi,minor);
  
  mapClient->SetDefaultTarget(gimnetID);
  
  mapClient->SetMapClientCallback(this);
  
  if (mapClient->Open() == MaCI::KMaCIOK)
  {
    opened=true;
  } else {
    opened=false;
    delete mapClient;
    mapClient=NULL;
  }
  return opened;
}

void CMapDisplay::Disconnect()
{
  
}
*/
/*
bool CMapDisplay::UpdateWholeMap(int timeout_ms)
{
  MaCI::Map::CMapData md;
  
  if (mapClient->GetMap(md,timeout_ms))
  {
  ownMutex_Lock(mapperMutex);
  mapper.Update(md,updatedGroundBlocks,updatedOccuBlocks);
  ownMutex_Unlock(mapperMutex);
  return true;
  }
  return false;
}*/

void CMapDisplay::UpdateWholeMap(MaCI::Map::CMapData &mapData)
{
  ownMutex_Lock(mapperMutex);
  mapper.Update(mapData,updatedGroundBlocks,updatedOccuBlocks);
  ownMutex_Unlock(mapperMutex);
}

bool CMapDisplay::SetDetailLevel(EMapDetailLevel level)
{
  if (level==KHighDetail || level==KLowDetail)
  {
    detailLevel=level;
    return true;
  }
  return false;
}

void CMapDisplay::SetOccupancyThreshold(float threshold)
{
  if (threshold<0)
  {
    occuThreshold=0;
  } else if (threshold>1.0) {
    threshold=1.0;
  } else {
    threshold=threshold;
  }
  
}

void CMapDisplay::OnMapDataEvent(const MaCI::Map::CMapData &aData, 
                    const unsigned int aDataSequence, 
                    MaCI::Map::CMapClient &aMapClient)
{
  ownMutex_Lock(mapperMutex);
  mapper.Update(aData,updatedGroundBlocks,updatedOccuBlocks);
  ownMutex_Unlock(mapperMutex);
}

void CMapDisplay::createHeightMapDisplayList(const TGroundModelBlock* block)
{
  blockData* bd=(blockData*)(block->userData);
  if (!bd)
  {
    bd=new blockData;
    block->userData=bd;
  }
  
  if (!(bd->highDetailList))
    bd->highDetailList=glGenLists(1);
  
  if (!(bd->lowDetailList))
    bd->lowDetailList=glGenLists(1);
  
  glNewList(bd->highDetailList,GL_COMPILE);
  
  drawHeightMapBlock(KHighDetail,block);
  
  glEndList();
  
  glNewList(bd->lowDetailList,GL_COMPILE);
  
  drawHeightMapBlock(KLowDetail,block);
  
  glEndList();
  
}

void CMapDisplay::createOccuDisplayList(const TOccuBlock* block)
{
  blockData* bd=(blockData*)(block->userData);
  if (!bd)
  {
    bd=new blockData;
    block->userData=bd;
  }
  
  if (!(bd->highDetailList))
    bd->highDetailList=glGenLists(1);
  
  if (!(bd->lowDetailList))
    bd->lowDetailList=glGenLists(1);
  
  glNewList(bd->highDetailList,GL_COMPILE);
  
  drawOccuBlock(KHighDetail,block);
  
  glEndList();
  
  glNewList(bd->lowDetailList,GL_COMPILE);
  
  drawOccuBlock(KLowDetail,block);
  
  glEndList();
}

void CMapDisplay::drawHeightMapBlock(int samplesPerQuad, const TGroundModelBlock* block)
{
  float reso=mapper.GetResolution();
  
  int samples_x=GROUND_MODEL_BLOCK_SIZE;
  int samples_y=GROUND_MODEL_BLOCK_SIZE;
          
  float pos_x=block->x;
  float pos_y=-block->y; //TODO: why this inverting is needed?? data from server is oriented wrong?
  
  float h;
  
  for (int y=0; y<samples_y-samplesPerQuad; y+=samplesPerQuad)
  {
    
    glBegin(GL_QUAD_STRIP);
    
    for (int x=0;x<samples_x; x+=samplesPerQuad)
    {
      h=block->data[y*samples_x+x];
      glColor3f(h,h,h);
      glVertex3f(pos_x+x*reso,pos_y-y*reso,z);
      
      h=block->data[(y+samplesPerQuad)*samples_x+x];
      glColor3f(h,h,h);
      glVertex3f(pos_x+x*reso,pos_y-(y+samplesPerQuad)*reso,z);
      
    }
    
    h=block->data[y*samples_x+samples_x-samplesPerQuad];
    glColor3f(h,h,h);
    glVertex3f(pos_x+samples_x*reso,pos_y-y*reso,z);
      
    h=block->data[(y+samplesPerQuad)*samples_x+samples_x-samplesPerQuad];
    glColor3f(h,h,h);
    glVertex3f(pos_x+samples_x*reso,pos_y-(y+samplesPerQuad)*reso,z);
    
    glEnd();
  }
  
  //draw the last strip, take values from next block
  glBegin(GL_QUAD_STRIP);
  
  int y=samples_y-samplesPerQuad;
  for (int x=0;x<samples_x; x+=samplesPerQuad)
  {
    h=block->data[y*samples_x+x];
    glColor3f(h,h,h);
    glVertex3f(pos_x+x*reso,pos_y-y*reso,z);
      
    //h=block->data[(y+samplesPerQuad)*samples_x+x];
    h=mapper.GetHeightMapValue(pos_x+x*reso,-pos_y+(y+samplesPerQuad)*reso);
    glColor3f(h,h,h);
    glVertex3f(pos_x+x*reso,pos_y-(y+samplesPerQuad)*reso,z);
  }
  
  //h=block->data[y*samples_x+samples_x-samplesPerQuad];
  h=mapper.GetHeightMapValue(pos_x+samples_x*reso,-pos_y+y*reso);
  glColor3f(h,h,h);
  glVertex3f(pos_x+samples_x*reso,pos_y-y*reso,z);
      
  //h=block->data[(y+samplesPerQuad)*samples_x+samples_x-samplesPerQuad];
  h=mapper.GetHeightMapValue(pos_x+samples_x*reso,-pos_y+(y+samplesPerQuad)*reso);
  glColor3f(h,h,h);
  glVertex3f(pos_x+samples_x*reso,pos_y-(y+samplesPerQuad)*reso,z);
  
  glEnd();
}
    
void CMapDisplay::drawOccuBlock(int samplesPerQuad, const TOccuBlock* block)
{
  float reso=mapper.GetResolution();
  
  int samples_x=GROUND_MODEL_BLOCK_SIZE;
  int samples_y=GROUND_MODEL_BLOCK_SIZE;
          
  float pos_x=block->x;
  float pos_y=-block->y; //TODO: why this inverting?? data from server is oriented wrong?
  
  for (int y=0; y<samples_y-samplesPerQuad; y+=samplesPerQuad)
  {
    unsigned char v;
    
    glBegin(GL_QUADS);
    for (int x=0;x<samples_x;x+=samplesPerQuad)
    {
      v=(unsigned char)(block->data[y*samples_x+x]);
      const float fv = v / 255.00;
      if (fv >= 0.5 + occuThreshold || fv <= 0.5 - occuThreshold) {
        // fv is now from 0-0.4x or 0.5x-1
        const float fvr = (0.2+fv)/1.2;

        glColor3f(fvr, fvr, fvr);

        glVertex3f(pos_x+x*reso,pos_y-y*reso,z);
        glVertex3f(pos_x+x*reso,pos_y-(y+samplesPerQuad)*reso,z);
        glVertex3f(pos_x+(x+samplesPerQuad)*reso,pos_y-(y+samplesPerQuad)*reso,z);
        glVertex3f(pos_x+(x+samplesPerQuad)*reso,pos_y-y*reso,z);
      } else {
        // Do nothing ;)
      }
    }
    
    
    glEnd();
  }
}

bool CMapDisplay::isDrawn(const TGroundModelBlock* block)
{
  //TODO:testing if block is inside viewable area
  
  if (block->userData)
    return true;
  return false;
}

bool CMapDisplay::isDrawn(const TOccuBlock* block)
{
  if (block->userData)
    return true;
  return false;
}

void CMapDisplay::displayHeightMapBlock(const TGroundModelBlock* block)
{
  
  blockData* bd=(blockData*)(block->userData);
  displayBlock(bd);
}

void CMapDisplay::displayOccuBlock(const TOccuBlock* block)
{
  blockData* bd=(blockData*)(block->userData);
  displayBlock(bd);
}

void CMapDisplay::displayBlock(blockData* bd)
{
  if (bd)
  {
    if (detailLevel==KHighDetail && bd->highDetailList)
    {
      glCallList(bd->highDetailList);
    } else if (detailLevel==KLowDetail && bd->lowDetailList) {
      glCallList(bd->lowDetailList);
    }
  }
}
